From e01a80d9b5aa7e995a88fc28d66ed4501cbf8da0 Mon Sep 17 00:00:00 2001 From: tsteven4 <13596209+tsteven4@users.noreply.github.com> Date: Sat, 12 Aug 2023 05:54:23 -0600 Subject: [PATCH] use efficient methods to delete trkpts in track filter. (#1155) * use efficient methods to delete trkpts in track filter. * clean up some code issues flagged by windows tools. --- trackfilter.cc | 198 +++++++++++++++++++++++-------------------------- trackfilter.h | 2 + 2 files changed, 95 insertions(+), 105 deletions(-) diff --git a/trackfilter.cc b/trackfilter.cc index 77ec41aca..1c3dd1384 100644 --- a/trackfilter.cc +++ b/trackfilter.cc @@ -22,16 +22,15 @@ static constexpr bool TRACKF_DBG = false; +#include // for_each, sort, stable_sort #include // for assert #include // for nan #include // for printf #include // for abs #include // for strlen, strchr, strcmp -#include // for gmtime, strftime +#include // for gmtime, strftime, time_t, tm #include // for next -#include // for sort, stable_sort - #include // for QByteArray #include // for QChar #include // for QDate @@ -195,10 +194,6 @@ void TrackFilter::trackfilter_fill_track_list_cb(const route_head* track) /* ca fatal(FatalMsg() << "track: name option is an invalid expression."); } if (!regex.match(track->rte_name).hasMatch()) { - foreach (Waypoint* wpt, track->waypoint_list) { - track_del_wpt(const_cast(track), wpt); - delete wpt; - } track_del_head(const_cast(track)); return; } @@ -275,7 +270,7 @@ void TrackFilter::trackfilter_pack_init_rte_name(route_head* track, const gpsbab if (track->rte_waypt_empty()) { dt = default_time; } else { - auto* wpt = track->waypoint_list.front(); + const auto* wpt = track->waypoint_list.front(); dt = wpt->GetCreationTime(); } time_t t = dt.toTime_t(); @@ -329,11 +324,20 @@ void TrackFilter::trackfilter_pack() route_head* master = track_list.first(); + if (!master->waypoint_list.empty()) { + std::for_each(std::next(master->waypoint_list.cbegin()), master->waypoint_list.cend(), + [](Waypoint* wpt)->void {wpt->wpt_flags.new_trkseg = 0;}); + } + while (track_list.size() > 1) { route_head* curr = track_list.takeAt(1); - foreach (Waypoint* wpt, curr->waypoint_list) { - track_del_wpt(curr, wpt); + // Steal all the waypoints + WaypointList curr_wpts; + track_swap_wpts(curr, curr_wpts); + // And add them to the master + foreach (Waypoint* wpt, curr_wpts) { + wpt->wpt_flags.new_trkseg = 0; track_add_wpt(master, wpt); } track_del_head(curr); @@ -357,15 +361,16 @@ void TrackFilter::trackfilter_merge() auto it = track_list.begin(); while (it != track_list.end()) { /* put all points into temp buffer */ route_head* track = *it; - foreach (Waypoint* wpt, track->waypoint_list) { - track_del_wpt(track, wpt); /* copies any new_trkseg flag forward, and clears new_trkseg flag. */ + // steal all the wpts + WaypointList wpts; + track_swap_wpts(track, wpts); + // add them to the buff or delete them + foreach (Waypoint* wpt, wpts) { if (wpt->creation_time.isValid()) { // we will put the merged points in one track segment, // as it isn't clear how track segments in the original tracks // should relate to the merged track. - // track_del_wpt cleared new_trkseg flag for wpt. - // track_add_wpt will set new_trkseg for the first point - // added to a track. + wpt->wpt_flags.new_trkseg = 0; buff.append(wpt); } else { delete wpt; @@ -381,7 +386,7 @@ void TrackFilter::trackfilter_merge() std::stable_sort(buff.begin(), buff.end(), trackfilter_merge_sort_cb); - Waypoint* prev = nullptr; + const Waypoint* prev = nullptr; for (auto* wpt : buff) { if ((prev == nullptr) || (prev->GetCreationTime() != wpt->GetCreationTime())) { @@ -416,16 +421,13 @@ void TrackFilter::trackfilter_split() fatal(MYNAME "-split: Cannot split more than one track, please pack (or merge) before!\n"); } else if (!track_list.isEmpty()) { route_head* master = track_list.first(); - int count = master->rte_waypt_ct(); + if (master->rte_waypt_ct() <= 1) { + return; + } - int i, j; double interval = -1; /* seconds */ double distance = -1; /* meters */ - if (count <= 1) { - return; - } - /* check additional options */ opt_interval = (opt_split && (strlen(opt_split) > 0) && (0 != strcmp(opt_split, TRACKFILTER_SPLIT_OPTION))); @@ -495,38 +497,40 @@ void TrackFilter::trackfilter_split() } } - QList buff; + // steal all the waypoints + WaypointList buff; + track_swap_wpts(master, buff); + assert(!buff.empty()); // enforced above - foreach (Waypoint* wpt, master->waypoint_list) { - buff.append(wpt); - } + trackfilter_split_init_rte_name(master, buff.front()->GetCreationTime()); - trackfilter_split_init_rte_name(master, buff.at(0)->GetCreationTime()); + route_head* curr = master; /* will be reset by first new track */ - route_head* curr = nullptr; /* will be set by first new track */ + // add the first waypoint to the first track + track_add_wpt(curr, buff.front()); + // and add subsequent waypoints to the first track or a new track + for (auto prev_it = buff.cbegin(), it = std::next(buff.cbegin()); it != buff.cend(); ++prev_it, ++it) { + const Waypoint* prev_wpt = *prev_it; + Waypoint* wpt = *it; - for (i=0, j=1; jGetCreationTime().toLocalTime().date() != - buff.at(j)->GetCreationTime().toLocalTime().date(); +// FIXME: This whole function needs to be reconsidered for arbitrary time. + new_track_flag = prev_wpt->GetCreationTime().toLocalTime().date() != + wpt->GetCreationTime().toLocalTime().date(); if constexpr(TRACKF_DBG) { if (new_track_flag) { - printf(MYNAME ": new day %s\n", qPrintable(buff.at(j)->GetCreationTime().toLocalTime().date().toString(Qt::ISODate))); + printf(MYNAME ": new day %s\n", qPrintable(wpt->GetCreationTime().toLocalTime().date().toString(Qt::ISODate))); } } } else { new_track_flag = true; if (distance > 0) { - double rt1 = RAD(buff.at(i)->latitude); - double rn1 = RAD(buff.at(i)->longitude); - double rt2 = RAD(buff.at(j)->latitude); - double rn2 = RAD(buff.at(j)->longitude); - double curdist = gcdist(rt1, rn1, rt2, rn2); - curdist = radtometers(curdist); + double curdist = radtometers( + gcdist(RAD(prev_wpt->latitude), RAD(prev_wpt->longitude), + RAD(wpt->latitude), RAD(wpt->longitude))); if (curdist <= distance) { new_track_flag = false; } else if constexpr(TRACKF_DBG) { @@ -535,7 +539,7 @@ void TrackFilter::trackfilter_split() } if (interval > 0) { - double tr_interval = 0.001 * buff.at(i)->GetCreationTime().msecsTo(buff.at(j)->GetCreationTime()); + double tr_interval = 0.001 * prev_wpt->GetCreationTime().msecsTo(wpt->GetCreationTime()); if (tr_interval <= interval) { new_track_flag = false; } else if constexpr(TRACKF_DBG) { @@ -549,14 +553,12 @@ void TrackFilter::trackfilter_split() printf(MYNAME ": splitting new track\n"); } curr = new route_head; - trackfilter_split_init_rte_name(curr, buff.at(j)->GetCreationTime()); + trackfilter_split_init_rte_name(curr, wpt->GetCreationTime()); track_add_head(curr); track_list.append(curr); } - if (curr != nullptr) { - track_del_wpt(master, buff.at(j)); - track_add_wpt(curr, buff.at(j)); - } + wpt->wpt_flags.new_trkseg = 0; + track_add_wpt(curr, wpt); } } } @@ -728,10 +730,11 @@ void TrackFilter::trackfilter_range() } if (!inside) { - track_del_wpt(track, wpt); - delete wpt; + wpt->wpt_flags.marked_for_deletion = 1; } } + // delete marked wpts + track_del_marked_wpts(track); if (track->rte_waypt_empty()) { track_del_head(track); @@ -756,12 +759,17 @@ void TrackFilter::trackfilter_seg2trk() QList new_track_list; for (auto* src : qAsConst(track_list)) { new_track_list.append(src); - route_head* dest = nullptr; + route_head* dest = src; route_head* insert_point = src; int trk_seg_num = 1; bool first = true; - foreach (Waypoint* wpt, src->waypoint_list) { + // steal all the waypoints from the src + WaypointList src_wpts; + track_swap_wpts(src, src_wpts); + + // and add them back to the original or a new route_head. + foreach (Waypoint* wpt, src_wpts) { if (wpt->wpt_flags.new_trkseg && !first) { dest = new route_head; @@ -778,18 +786,7 @@ void TrackFilter::trackfilter_seg2trk() new_track_list.append(dest); } - /* If we found a track separator, transfer from original to - * new track. We have to reset new_trkseg temporarily to - * prevent track_del_wpt() from copying it to the next track - * point. - */ - if (dest) { - unsigned orig_new_trkseg = wpt->wpt_flags.new_trkseg; - wpt->wpt_flags.new_trkseg = 0; - track_del_wpt(src, wpt); - wpt->wpt_flags.new_trkseg = orig_new_trkseg; - track_add_wpt(dest, wpt); - } + track_add_wpt(dest, wpt); first = false; } } @@ -809,13 +806,12 @@ void TrackFilter::trackfilter_trk2seg() while (track_list.size() > 1) { route_head* curr = track_list.takeAt(1); + // steal all the wpts + WaypointList curr_wpts; + track_swap_wpts(curr, curr_wpts); + // and add them to the master bool first = true; - foreach (Waypoint* wpt, curr->waypoint_list) { - - unsigned orig_new_trkseg = wpt->wpt_flags.new_trkseg; - wpt->wpt_flags.new_trkseg = 0; - track_del_wpt(curr, wpt); - wpt->wpt_flags.new_trkseg = orig_new_trkseg; + foreach (Waypoint* wpt, curr_wpts) { track_add_wpt(master, wpt); if (first) { wpt->wpt_flags.new_trkseg = 1; @@ -888,18 +884,11 @@ void TrackFilter::trackfilter_faketime() bool TrackFilter::trackfilter_points_are_same(const Waypoint* wpta, const Waypoint* wptb) { - // We use a simpler (non great circle) test for lat/lon here as this - // is used for keeping the 'bookends' of non-moving points. - // - // Latitude spacing is about 27 feet per .00001 degree. - // Longitude spacing varies, but the reality is that anything closer - // than 27 feet does little but clutter the output. - // As this is about the limit of consumer grade GPS, it seems a - // reasonable tradeoff. - return - std::abs(wpta->latitude - wptb->latitude) < .00001 && - std::abs(wpta->longitude - wptb->longitude) < .00001 && + radtometers(gcdist(RAD(wpta->latitude), + RAD(wpta->longitude), + RAD(wptb->latitude), + RAD(wptb->longitude))) < kDistanceLimit && std::abs(wpta->altitude - wptb->altitude) < 20 && wpta->courses_equal(*wptb) && wpta->speeds_equal(*wptb) && @@ -911,46 +900,45 @@ bool TrackFilter::trackfilter_points_are_same(const Waypoint* wpta, const Waypoi void TrackFilter::trackfilter_segment_head(const route_head* rte) { double avg_dist = 0; - int index = 0; - Waypoint* prev_wpt = nullptr; - // Consider tossing trackpoints closer than this in radians. - // (Empirically determined; It's a few dozen feet.) - const double ktoo_close = 0.000005; + const Waypoint* prev_wpt = nullptr; - const auto wptlist = rte->waypoint_list; // track_del_wpt will modify rte->waypoint_list + const auto wptlist = rte->waypoint_list; for (auto it = wptlist.cbegin(); it != wptlist.cend(); ++it) { auto* wpt = *it; - if (index > 0) { - double cur_dist = gcdist(RAD(prev_wpt->latitude), - RAD(prev_wpt->longitude), - RAD(wpt->latitude), - RAD(wpt->longitude)); - // Denoise points that are on top of each other. - if (avg_dist == 0) { - avg_dist = cur_dist; - } - - if (cur_dist < ktoo_close) { - if (wpt != wptlist.back()) { - auto* next_wpt = *std::next(it); + if (it != wptlist.cbegin()) { + double cur_dist = radtometers(gcdist(RAD(prev_wpt->latitude), + RAD(prev_wpt->longitude), + RAD(wpt->latitude), + RAD(wpt->longitude))); + + // Denoise points that are on top of each other, + // keeping the first and last of the group. + if (cur_dist < kDistanceLimit) { + if (auto next_it = std::next(it); next_it != wptlist.cend()) { + const auto* next_wpt = *next_it; if (trackfilter_points_are_same(prev_wpt, wpt) && trackfilter_points_are_same(wpt, next_wpt)) { - track_del_wpt(const_cast(rte), wpt); - delete wpt; - continue; + wpt->wpt_flags.marked_for_deletion = 1; + continue; // without updating prev_wpt, the first in the group. } } } - if (cur_dist > .001 && cur_dist > 1.2* avg_dist) { - avg_dist = cur_dist = 0; + + if (avg_dist == 0) { + avg_dist = cur_dist; + } + + if (cur_dist > 6378.14 && cur_dist > 1.2 * avg_dist) { + avg_dist = 0; wpt->wpt_flags.new_trkseg = 1; + } else { + // Update weighted moving average; + avg_dist = (cur_dist + 4.0 * avg_dist) / 5.0; } - // Update weighted moving average; - avg_dist = (cur_dist + 4.0 * avg_dist) / 5.0; } prev_wpt = wpt; - index++; } + track_del_marked_wpts(const_cast(rte)); } /******************************************************************************* diff --git a/trackfilter.h b/trackfilter.h index 699146b86..e00dffba3 100644 --- a/trackfilter.h +++ b/trackfilter.h @@ -24,6 +24,7 @@ #include // for QDateTime #include // for QList +#include // for QString #include // for QVector #include // for qint64 @@ -56,6 +57,7 @@ private: /* Constants */ + static constexpr double kDistanceLimit = 1.11319; // for points to be considered the same, meters. static constexpr char TRACKFILTER_PACK_OPTION[] = "pack"; static constexpr char TRACKFILTER_SPLIT_OPTION[] = "split"; static constexpr char TRACKFILTER_SDIST_OPTION[] = "sdistance"; -- 2.30.2